#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     DCArmLift,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_2,    WristLift,            tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    Claw,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

#define SERVOS_MAX_VALUE 250
#define SERVOS_MIN_VALUE 0
#define SERVOS_WAIT_TIME_10MSEC 1

int servo1Pos = 0;
int servo2Pos = 0;
int servo3Pos = 0;
int servo4Pos = 0;
int servo5Pos = 0;
int servo6Pos = 0;

void displayDebugData(int value)
{
	nxtDisplayClearTextLine(2);
	nxtDisplayTextLine(2, "%d", value);
	while(nNxtButtonPressed != 3)
		NULL;
	nxtDisplayClearTextLine(2);
}

void initializeRobot()
{
	servo1Pos = ServoValue[servo1];
	servo2Pos = ServoValue[servo2];
	servo3Pos = ServoValue[servo3];
	servo4Pos = ServoValue[servo4];
	servo5Pos = ServoValue[servo5];
	servo6Pos = ServoValue[servo6];
}

task main()
{
	int threshold = 50;
	initializeRobot();
  waitForStart();   // wait for start of tele-op phase

	while (true)
  {
  	getJoystickSettings(joystick);

  	/*/ ARM LIFT
  	{
  		if (joystick.joy1_y1 > threshold)
  		{
  			servoTarget[ArmLift] = 256;  // FORWARD
  			//if (ServoValue[ArmLift] < (SERVOS_MAX_VALUE*4)+servo1StartPos)
  			//	servo1Pos = servo1Pos+1;
  		}
  		else if (joystick.joy1_y1 < threshold*-1)
  		{
  			servoTarget[ArmLift] = 0;    // BACKWARDS
  			//if (servo1Pos > SERVOS_MIN_VALUE+servo1StartPos)
  			//	servo1Pos = servo1Pos-1;
  		}
  		else
  		{
  			servoTarget[ArmLift] = 127; // STOP
  		}

  		wait10Msec(SERVOS_WAIT_TIME_10MSEC);
  	}*/


  	if(abs(joystick.joy1_y1) > threshold)
		{
  		if (joystick.joy1_y1 > 0)
  		{
  			motor[DCArmLift] = 20;
  		}
  		else if (joystick.joy1_y1 < 0)
  		{
  			motor[DCArmLift] = -20;
  		}
  	}
  	else
  	{
  		motor[DCArmLift] = 5;
  	}


  	// This where WristLift begins///////////////
		if(abs(joystick.joy1_y2) > threshold)
		{
  		if (joystick.joy1_y2 > 0)
  		{
  			if (servo2Pos < SERVOS_MAX_VALUE)
  				servo2Pos = servo2Pos+5;
  		}
  		else if (joystick.joy1_y2 < 0)
  		{
  			if (servo2Pos > SERVOS_MIN_VALUE)
  				servo2Pos = servo2Pos-5;
  		}
  		servoTarget[WristLift] = servo2Pos;
  		wait10Msec(SERVOS_WAIT_TIME_10MSEC);
  	}
//This is where Claw begins/////////////////
  	{
			if(joy1Btn(1))
			{
				if (ServoValue[Claw] < SERVOS_MAX_VALUE)
					servo3Pos = servo3Pos+5;
			}
			if (joy1Btn(3))
			{
				if (ServoValue[Claw] > SERVOS_MIN_VALUE)
					servo3Pos = servo3Pos-5;
			}
			servoTarget[Claw] = servo3Pos;
			wait10Msec(SERVOS_WAIT_TIME_10MSEC);
  	}




  	/*
  	if(abs(joystick.joy1_y2) > threshold )
  	{
  		if (joystick.joy1_y2 > 0 && ServoValue[servo3] < servo3OriginalPos+25)
  		{
  			servo3Pos = servo3Pos+1;
  		}
  		if (joystick.joy1_y2 < 0 && ServoValue[servo3] > servo3OriginalPos)
  		{
  			servo3Pos = servo3Pos-1;
  		}
  		servoTarget[servo3] = servo3Pos;
  		wait10Msec(SERVOS_WAIT_TIME_10MSEC);
  	}
  	if(abs(joystick.joy1_y1) > threshold)
  	{
  		if (joystick.joy1_y1 > 0)
  		{
  			if (ServoValue[servo1] < SERVOS_MAX_VALUE)
  				servo1Pos = servo1Pos+1;
  			if (ServoValue[servo2] > SERVOS_MIN_VALUE)
  				servo2Pos = servo2Pos-1;
  		}
  		else if (joystick.joy1_y1 < 0)
  		{
  			if (ServoValue[servo1] > SERVOS_MIN_VALUE)
  				servo1Pos = servo1Pos-1;
  			if (ServoValue[servo2] < SERVOS_MAX_VALUE)
  				servo2Pos = servo2Pos+1;
  		}
  		servoTarget[servo1] = servo1Pos;
  		servoTarget[servo2] = servo2Pos;
  		wait10Msec(SERVOS_WAIT_TIME_10MSEC);
  	}
*/
  }
}
